﻿using System;
using System.Text;
using System.Windows;
using System.Windows.Media;
using System.Windows.Shapes;
using Microsoft.Research.Kinect.Nui;
using System.Threading;

namespace FLKRP.WP7.Server
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        Server.ClientManager _clientManager;

        Runtime _kinect;
        int _frameCount;

        public MainWindow()
        {
            InitializeComponent();
            _clientManager = new Server.ClientManager();
            _clientManager.Start();

            DataContext = _clientManager;

            this.Loaded += MainWindow_Loaded;
        }

        void MainWindow_Loaded(object sender, RoutedEventArgs e)
        {
            _kinect = new Runtime();

            try
            {
                _kinect.Initialize(RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking | RuntimeOptions.UseColor);
            }
            catch (InvalidOperationException ex)
            {
                MessageBox.Show("Failed connecting to Kinect, please make sure it is plugged in to a primary USB port on your computer\r\n\r\nIn some cases, Kinect doesn't work well when plugged in to a USB hub.\r\n\r\n" + ex.Message);
                return;
            }

            try
            {
                _kinect.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
                _kinect.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);
            }
            catch (InvalidOperationException)
            {
                System.Windows.MessageBox.Show("Failed to open stream. Please make sure to specify a supported image type and resolution.");
                return;
            }

            _kinect.SkeletonFrameReady += _kinect_SkeletonFrameReady;
        }

        private Point getDisplayPosition(Joint joint)
        {
            float depthX, depthY;
            _kinect.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
            depthX = Math.Max(0, Math.Min(depthX * 320, 320));  //convert to 320, 240 space
            depthY = Math.Max(0, Math.Min(depthY * 240, 240));  //convert to 320, 240 space
            int colorX, colorY;
            ImageViewArea iv = new ImageViewArea();
            // only ImageResolution.Resolution640x480 is supported at this point
            _kinect.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);
            // map back to skeleton.Width & skeleton.Height
            return new Point((int)(Skeleton.ActualWidth * colorX / 640.0), (int)(Skeleton.ActualHeight * colorY / 480));
        }

        Polyline getBodySegment(JointsCollection joints, Brush brush, params JointID[] ids)
        {
            PointCollection points = new PointCollection(ids.Length);
            for (int i = 0; i < ids.Length; ++i)
                points.Add(getDisplayPosition(joints[ids[i]]));

            Polyline polyline = new Polyline { Points = points, Stroke = brush, StrokeThickness = 5 };
            return polyline;
        }


        private void addBone(StringBuilder output, Polyline line)
        {
            Skeleton.Children.Add(line);

            output.Append(string.Format("{{'color':'{0}','points':[", line.Stroke.ToString()));
            var idx = 0;
            foreach (var point in line.Points)
            {
                output.Append(string.Format("{0}{{'x':{1},'y':{2}}}", idx++ > 0 ? "," : "", point.X, point.Y));
            }
            output.Append("]},");
        }

        public void addJoint(StringBuilder output, Line line)
        {
            Skeleton.Children.Add(line);

            output.Append(string.Format("{{'color':'{0}','x1':{1},'y1':{2},'x2':{3},'y2':{4}}},", line.Stroke.ToString(), line.X1, line.Y1, line.X2, line.Y2));
        }

        private void sendModel(string output)
        {
            ThreadPool.QueueUserWorkItem((obj) =>
            {
                _clientManager.SendData(output);
            });
        }

        void _kinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            var bones = new StringBuilder("[");
            var joints = new StringBuilder("[");

            SkeletonFrame skeletonFrame = e.SkeletonFrame;
            int iSkeleton = 0;

            var actualData = 0;
            Skeleton.Children.Clear();
            foreach (SkeletonData data in skeletonFrame.Skeletons)
            {
                if (SkeletonTrackingState.Tracked == data.TrackingState)
                {
                    // Draw bones
                    Brush brush = Common.SkeletalColors[iSkeleton % Common.SkeletalColors.Length];
                    addBone(bones, getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.Spine, JointID.ShoulderCenter, JointID.Head));
                    addBone(bones, getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderLeft, JointID.ElbowLeft, JointID.WristLeft, JointID.HandLeft));
                    addBone(bones, getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderRight, JointID.ElbowRight, JointID.WristRight, JointID.HandRight));
                    addBone(bones, getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipLeft, JointID.KneeLeft, JointID.AnkleLeft, JointID.FootLeft));
                    addBone(bones, getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipRight, JointID.KneeRight, JointID.AnkleRight, JointID.FootRight));

                    // Draw joints
                    foreach (Joint joint in data.Joints)
                    {
                        Point jointPos = getDisplayPosition(joint);
                        Line jointLine = new Line();
                        jointLine.X1 = jointPos.X - 3;
                        jointLine.X2 = jointLine.X1 + 6;
                        jointLine.Y1 = jointLine.Y2 = jointPos.Y;
                        jointLine.Stroke = Common.JointColors[joint.ID];
                        jointLine.StrokeThickness = 6;
                        addJoint(joints, jointLine);
                    }
                    actualData++;
                }
                iSkeleton++;
            }

            bones.Remove(bones.Length - 1, 1);
            joints.Remove(joints.Length - 1, 1);

            bones.Append("]");
            joints.Append("]");

            if (++_frameCount > 5)
            {
                _frameCount = 0;

                var message = string.Format("{{'bones':{0},'joints':{1}}}", bones, joints);
                sendModel(message);
            }
        }



        protected override void OnClosing(System.ComponentModel.CancelEventArgs e)
        {
            _clientManager.Stop();
            _kinect.Uninitialize();

            e.Cancel = false;

            base.OnClosing(e);
        }
    }
}
